% Pose estimation frame test 

A = [0 0 ]';
B = [177 0]';
C = [177 177 ]';
D = [0 177]';

% FRAME 01:
% import cam para, M matrix

% Enter H from ret
% H = [0.000034200095140  -0.002152942205440   0.846292631040025
%      0.002264656354052  -0.000013139174776   0.532691808370370
%      0.000000545352868   0.000000294728233   0.004295901680406]

ptSample(1,1:4) = [211 209 391 395];
ptSample(2,1:4) = [237 52 46 233];

H = HomoMtrxCalc(pose, ptSample)

% H = [-0.000004379737249  -0.003198364540768  -0.664940158033976
%       0.003283860900980   0.000117016039949  -0.746875912102011
%      -0.000000191332517   0.000000196560182  -0.003151375156559]

s = norm(M\H(:,1));
ld = 1/s
 
r1 = ld*(M\H(:,1))
% r1 = M\H(:,1)

r2 = ld*(M\H(:,2))
% r2 = M\H(:,2)

r3 = cross(r1,r2)


R = [r1 r2 r3]

[U,S,V] = svd(R);

R_dot = U*eye(3)*V';

t = ld*(M\H(:,3))
% t = M\H(:,3)
% /2

angles = tr2rpy_Tsai(R_dot)/pi*180

input('Press Enter');

% OUTPUT: 

% t =
% 1.0e+002 *
%   -0.827657341769193
%   -0.855306145785722
%    7.457720941261587
% angles =
%   10.603550762874546   5.993047737805962  93.055981052707281

% manual computation
% t =
%   1.0e+002 *
%    0.667597222184084
%   -0.243371704270155
%   -7.208347910353948
% angles =
%   -2.468603730152175   2.691448822342820  89.128413868658342

% FRAME 02:

% import cam para, M matrix

% Enter H from ret
% H = [  0.000034056733040  -0.001622538913696   0.876854532604663
%        0.002387354052232   0.000289666200012   0.480731651647286
%        0.000000436241274   0.000003120424928   0.003845853213178]

ptSample(1,1:4) = [264 265 452 458];
ptSample(2,1:4) = [238 45 10 232];

H = HomoMtrxCalc(pose, ptSample)

%   H = [0.000155207321038   0.002094646651326   0.742726166521986
%       -0.003044014555361  -0.000596305381577   0.669578892546738
%        0.000000525708115  -0.000002159212751   0.002813356691371]

s = norm(M\H(:,1));
ld = 1/s
 
r1 = ld*(M\H(:,1))

r2 = ld*(M\H(:,2))

r3 = cross(r1,r2)


R = [r1 r2 r3]

[U,S,V] = svd(R);

R_dot = U*eye(3)*V';

t = ld*(M\H(:,3))

angles = tr2rpy_Tsai(R_dot)/pi*180

input('Press Enter');

% OUTPUT: 
% t =
% 1.0e+002 *
%   -0.443474513569679
%   -0.715058892974726
%    6.307101151012983
% angles =
%    6.126114665042393  43.871646818785493  92.033018580297892


% manual computation 

% t =
%   1.0e+002 *
%   -0.155464492364900
%    0.236187520419755
%    6.733555825560455
% angles =
%   -7.307119691731055  31.411073281700133 -89.290855095055022



% FRAME 03:

% import cam para, M matrix

% Enter H from ret
% H = [  0.000034056733040  -0.001622538913696   0.876854532604663
%        0.002387354052232   0.000289666200012   0.480731651647286
%        0.000000436241274   0.000003120424928   0.003845853213178]

ptSample(1,1:4) = [163 172 423 421];
ptSample(2,1:4) = [397 81 14 457];

H = HomoMtrxCalc(pose, ptSample)

%   H = [0.000155207321038   0.002094646651326   0.742726166521986
%       -0.003044014555361  -0.000596305381577   0.669578892546738
%        0.000000525708115  -0.000002159212751   0.002813356691371]

s = norm(M\H(:,1));
ld = 1/s
 
r1 = ld*(M\H(:,1))

r2 = ld*(M\H(:,2))

r3 = cross(r1,r2)


R = [r1 r2 r3]

[U,S,V] = svd(R);

R_dot = U*eye(3)*V';

t = ld*(M\H(:,3))

angles = tr2rpy_Tsai(R_dot)/pi*180









